namespace mathD
{
    export class quat extends Float32Array
    {

    }

    export class Quaternion
    {
        constructor(x: number = 0, y: number = 0, z: number = 0, w: number = 1)
        {
            this.x = x;
            this.y = y;
            this.z = z;
            this.w = w;
        }
        x: number;
        y: number;
        z: number;
        w: number;
        toString(): string
        {
            return this.x + "," + this.y + "," + this.z + "," + this.w;
        }

        static normalize(src: Quaternion, out: Quaternion)
        {
            var mag: number = 1 / Math.sqrt(src.x * src.x + src.y * src.y + src.z * src.z + src.w * src.w);

            out.x *= mag;
            out.y *= mag;
            out.z *= mag;
            out.w *= mag;
        }

        static transformVector(src: Quaternion, vector: Vector3, out: Vector3)
        {
            var x1: number, y1: number, z1: number, w1: number;
            var x2: number = vector.x, y2: number = vector.y, z2: number = vector.z;

            w1 = -src.x * x2 - src.y * y2 - src.z * z2;
            x1 = src.w * x2 + src.y * z2 - src.z * y2;
            y1 = src.w * y2 - src.x * z2 + src.z * x2;
            z1 = src.w * z2 + src.x * y2 - src.y * x2;

            out.x = -w1 * src.x + x1 * src.w - y1 * src.z + z1 * src.y;
            out.y = -w1 * src.y + x1 * src.z + y1 * src.w - z1 * src.x;
            out.z = -w1 * src.z - x1 * src.y + y1 * src.x + z1 * src.w;

        }
        static transformVectorDataAndQuat(src: Float32Array, srcseek: number, vector: Vector3, out: Vector3)
        {
            var x1: number, y1: number, z1: number, w1: number;
            var x2: number = vector.x, y2: number = vector.y, z2: number = vector.z;
            var srcx = src[srcseek]; var srcy = src[srcseek + 1]; var srcz = src[srcseek + 2]; var srcw = src[srcseek + 3];

            w1 = -srcx * x2 - srcy * y2 - srcz * z2;
            x1 = srcw * x2 + srcy * z2 - srcz * y2;
            y1 = srcw * y2 - srcx * z2 + srcz * x2;
            z1 = srcw * z2 + srcx * y2 - srcy * x2;

            out.x = -w1 * srcx + x1 * srcw - y1 * srcz + z1 * srcy;
            out.y = -w1 * srcy + x1 * srcz + y1 * srcw - z1 * srcx;
            out.z = -w1 * srcz - x1 * srcy + y1 * srcx + z1 * srcw;

        }
        static magnitude(src: Quaternion): number
        {
            return Math.sqrt(src.w * src.w + src.x * src.x + src.y * src.y + src.z * src.z);
        }

        static clone(src: Quaternion, out: Quaternion)
        {
            out.x = src.x;
            out.y = src.y;
            out.z = src.z;
            out.w = src.w;
        }
        static toMatrix(src: Quaternion, out: Matrix)
        {
            var xy2: number = 2.0 * src.x * src.y, xz2: number = 2.0 * src.x * src.z, xw2: number = 2.0 * src.x * src.w;
            var yz2: number = 2.0 * src.y * src.z, yw2: number = 2.0 * src.y * src.w, zw2: number = 2.0 * src.z * src.w;
            var xx: number = src.x * src.x, yy: number = src.y * src.y, zz: number = src.z * src.z, ww: number = src.w * src.w;

            out.rawData[0] = xx - yy - zz + ww;
            out.rawData[4] = xy2 - zw2;
            out.rawData[8] = xz2 + yw2;
            out.rawData[12] = 0;
            out.rawData[1] = xy2 + zw2;
            out.rawData[5] = -xx + yy - zz + ww;
            out.rawData[9] = yz2 - xw2;
            out.rawData[13] = 0;
            out.rawData[2] = xz2 - yw2;
            out.rawData[6] = yz2 + xw2;
            out.rawData[10] = -xx - yy + zz + ww;
            out.rawData[14] = 0;
            out.rawData[3] = 0.0;
            out.rawData[7] = 0.0;
            out.rawData[11] = 0;
            out.rawData[15] = 1;
        }

        static inverse(src: Quaternion, out: Quaternion)
        {
            var norm: number = src.w * src.w + src.x * src.x + src.y * src.y + src.z * src.z;

            if (norm > 0.0)
            {
                var invNorm = 1.0 / norm;
                out.w = src.w * invNorm;
                out.x = -src.x * invNorm;
                out.y = -src.y * invNorm;
                out.z = -src.z * invNorm;
            }
        }

        static fromYawPitchRoll(yaw: number, pitch: number, roll: number, result: Quaternion): void 
        {
            // Produces a quaternion from Euler angles in the z-y-x orientation (Tait-Bryan angles)
            var halfRoll = roll * 0.5;
            var halfPitch = pitch * 0.5;
            var halfYaw = yaw * 0.5;

            var sinRoll = Math.sin(halfRoll);
            var cosRoll = Math.cos(halfRoll);
            var sinPitch = Math.sin(halfPitch);
            var cosPitch = Math.cos(halfPitch);
            var sinYaw = Math.sin(halfYaw);
            var cosYaw = Math.cos(halfYaw);

            result.x = (cosYaw * sinPitch * cosRoll) + (sinYaw * cosPitch * sinRoll);
            result.y = (sinYaw * cosPitch * cosRoll) - (cosYaw * sinPitch * sinRoll);
            result.z = (cosYaw * cosPitch * sinRoll) - (sinYaw * sinPitch * cosRoll);
            result.w = (cosYaw * cosPitch * cosRoll) + (sinYaw * sinPitch * sinRoll);
        }
        static multiply(srca: Quaternion, srcb: Quaternion, out: Quaternion)
        {
            var w1: number = srca.w, x1: number = srca.x, y1: number = srca.y, z1: number = srca.z;
            var w2: number = srcb.w, x2: number = srcb.x, y2: number = srcb.y, z2: number = srcb.z;

            out.w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2;
            out.x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2;
            out.y = w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2;
            out.z = w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2;

            this.normalize(out, out);
        }
        static multiplyDataAndQuat(srca: Float32Array, srcaseek: number, srcb: Quaternion, out: Quaternion)
        {
            var w1: number = srca[srcaseek + 3], x1: number = srca[srcaseek + 0], y1: number = srca[srcaseek + 1], z1: number = srca[srcaseek + 2];
            var w2: number = srcb.w, x2: number = srcb.x, y2: number = srcb.y, z2: number = srcb.z;

            out.w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2;
            out.x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2;
            out.y = w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2;
            out.z = w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2;

            this.normalize(out, out);
        }
        static multiplyVector(vector: Vector3, scr: Quaternion, out: Quaternion)
        {
            var x2: number = vector.x;
            var y2: number = vector.y;
            var z2: number = vector.z;

            out.w = -scr.x * x2 - scr.y * y2 - scr.z * z2;
            out.x = scr.w * x2 + scr.y * z2 - scr.z * y2;
            out.y = scr.w * y2 - scr.x * z2 + scr.z * x2;
            out.z = scr.w * z2 + scr.x * y2 - scr.y * x2;
        }

        static lerp(srca: Quaternion, srcb: Quaternion, out: Quaternion, t: number)
        {
            var w1: number = srca.w, x1: number = srca.x, y1: number = srca.y, z1: number = srca.z;
            var w2: number = srcb.w, x2: number = srcb.x, y2: number = srcb.y, z2: number = srcb.z;

            if (w1 * w2 + x1 * x2 + y1 * y2 + z1 * z2 < 0)
            {
                w2 = -w2;
                x2 = -x2;
                y2 = -y2;
                z2 = -z2;
            }

            out.w = w1 + t * (w2 - w1);
            out.x = x1 + t * (x2 - x1);
            out.y = y1 + t * (y2 - y1);
            out.z = z1 + t * (z2 - z1);

            var len: number = 1.0 / Math.sqrt(out.w * out.w + out.x * out.x + out.y * out.y + out.z * out.z);
            out.w *= len;
            out.x *= len;
            out.y *= len;
            out.z *= len;
        }

        static fromAxisAngle(axis: Vector3, angle: number, out: Quaternion)
        {
            angle *= Math.PI / 180.0;
            var halfAngle: number = angle * 0.5;
            var sin_a: number = Math.sin(halfAngle);

            out.w = Math.cos(halfAngle);
            out.x = axis.x * sin_a;
            out.y = axis.y * sin_a;
            out.z = axis.z * sin_a;

            this.normalize(out, out);
        }

        static toAxisAngle(src: Quaternion, axis: Vector3): number
        {
            var sqrLength: number = src.x * src.x + src.y * src.y + src.z * src.z;
            var angle: number = 0;
            if (sqrLength > 0.0)
            {
                angle = 2.0 * Math.acos(src.w);
                sqrLength = 1.0 / Math.sqrt(sqrLength);
                axis.x = src.x * sqrLength;
                axis.y = src.y * sqrLength;
                axis.z = src.z * sqrLength;
            }
            else
            {
                angle = 0;
                axis.x = 1.0;
                axis.y = 0;
                axis.z = 0;
            }
            angle /= Math.PI / 180.0;
            return angle;
        }

        static fromEulerAngles(ax: number, ay: number, az: number, out: Quaternion)
        {
            ax *= Math.PI / 180;
            ay *= Math.PI / 180;
            az *= Math.PI / 180;

            var halfX: number = ax * 0.5, halfY: number = ay * 0.5, halfZ: number = az * 0.5;
            var cosX: number = Math.cos(halfX), sinX: number = Math.sin(halfX);
            var cosY: number = Math.cos(halfY), sinY: number = Math.sin(halfY);
            var cosZ: number = Math.cos(halfZ), sinZ: number = Math.sin(halfZ);

            out.w = cosX * cosY * cosZ + sinX * sinY * sinZ;
            out.x = sinX * cosY * cosZ + cosX * sinY * sinZ;
            out.y = cosX * sinY * cosZ - sinX * cosY * sinZ;
            out.z = cosX * cosY * sinZ - sinX * sinY * cosZ;

            this.normalize(out, out);
        }
        static toEulerAngles(src: Quaternion, out: Vector3)
        {
            var temp: number = 2.0 * (src.w * src.x - src.y * src.z);
            temp = mathD.floatClamp(temp, -1.0, 1.0);
            out.x = Math.asin(temp);

            out.y = Math.atan2(2.0 * (src.w * src.y + src.z * src.x), 1.0 - 2.0 * (src.y * src.y + src.x * src.x));

            out.z = Math.atan2(2.0 * (src.w * src.z + src.y * src.x), 1.0 - 2.0 * (src.x * src.x + src.z * src.z));

            out.x /= Math.PI / 180;
            out.y /= Math.PI / 180;
            out.z /= Math.PI / 180;
        }
        static reset(src: Quaternion)
        {
            src.x = 0;
            src.y = 0;
            src.z = 0;
            src.w = 1;
        }
        //获取一个注视目标的四元数


        static lookat(pos: Vector3, targetpos: Vector3, out: Quaternion,up:Vector3=pool.vector3_up)
        {
            var dir = mathD.pool.new_vector3();
            Vector3.subtract(targetpos, pos, dir);
            Vector3.normalize(dir, dir);
            var dot = Vector3.dot(mathD.pool.vector3_forward, dir);
            if (Math.abs(dot - (-1.0)) < 0.000001)
            {
                this.fromAxisAngle(mathD.pool.vector3_up, 180, out);
                return;
            }
            if (Math.abs(dot - 1.0) < 0.000001)
            {
                out.x = 0;
                out.y = 0;
                out.z = 0;
                out.w = 1;
                return;
            }
            dot = mathD.floatClamp(dot, -1, 1);
            var rotangle = Math.acos(dot);
            var rotAxis = mathD.pool.new_vector3();
            Vector3.cross(mathD.pool.vector3_forward, dir, rotAxis);
            Quaternion.fromAxisAngle(rotAxis, rotangle, out);
        }
        static lookat1(pos: Vector3, targetpos: Vector3, out: Quaternion,up:Vector3=pool.vector3_up)
        {
            var target_z=mathD.pool.new_vector3();
            mathD.Vector3.subtract(targetpos,pos,target_z);
            mathD.Vector3.normalize(target_z,target_z);

            var target_x=mathD.pool.new_vector3();
            mathD.Vector3.cross(up,target_z,target_x);
        }

        /**
         * from、to需要被normalized
         * @param from 
         * @param to 
         * @param out 
         * @param potentialAxis 
         */
        static formtoRotation(from:Vector3,to:Vector3,out:Quaternion,potentialAxis:Vector3=Vector3.up)
        {
            // Based on Stan Melax's article in Game Programming Gems
            //ogre      
           var dot=mathD.Vector3.dot(from,to);
           if(Math.abs(dot - 1.0) < 0.000001)//向量重合
           {
               out.w=1;
               out.x=out.y=out.z=0;
           }
           else if (Math.abs(dot - (-1.0)) < 0.000001)//向量反向
           {
                mathD.Quaternion.fromAxisAngle(potentialAxis,180,out);
           }
           else
           {    
                var cross=mathD.pool.new_vector3();
                mathD.Vector3.cross(from,to,cross);
                out.x=cross.x;
                out.y=cross.y;
                out.z=cross.z;
                out.w=1.0+dot;
                mathD.Quaternion.normalize(out,out);
           }
        }
    }
}